/**
 * @file
 * @author dousha 
 * @date 2021/5/25.
 */

#include "quaternionFilter.h"
#include <stdint.h>
#include <fastmath.h>
#include "qfplib-m3.h"
#include "solver.h"

#define Kp 2.0f * 5.0f
#define Ki 0.008f

static Triad acc, gyro, mag;
static float q0 = 1.f, q1 = 0.f, q2 = 0.f, q3 = 0.f;
static uint32_t now_update = 0, last_update = 0;
static float exInt = 0, eyInt = 0, ezInt = 0;
const float PI = 3.141592f;
const float RAD2DEG = 180.f / PI;
const float DEG2RAD = PI / 180.f;
const float GYRO_SENSITIVITY = 16.384f;
const float ACC_SENSITIVITY = 8192.f;

static void imuUpdate(Position *position) {
	float q0q0 = q0 * q0;
	float q1q1 = q1 * q1;
	float q2q2 = q2 * q2;
	float q3q3 = q3 * q3;

	float q0q1 = q0 * q1;
	float q0q2 = q0 * q2;
	float q0q3 = q0 * q3;
	float q1q2 = q1 * q2;
	float q1q3 = q1 * q3;
	float q2q3 = q2 * q3;

	float normalise;
	float ex, ey, ez;
	float halfT;
	float hx, hy, hz, bx, bz;
	float vx, vy, vz, wx, wy, wz;

	now_update = HAL_GetTick(); //单位ms
	halfT = ((float)(now_update - last_update) / 2000.0f);
	last_update = now_update;

	gyro.x *= DEG2RAD;	/*度转弧度*/
	gyro.y *= DEG2RAD;
	gyro.z *= DEG2RAD;

	/* 对加速度计数据进行归一化处理 */
	if(acc.x != 0 || acc.y != 0 || acc.z != 0)
	{
		normalise = qfp_fsqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
		acc.x /= normalise;
		acc.y /= normalise;
		acc.z /= normalise;
	}

	/* 对磁力计数据进行归一化处理 */
	if(mag.x != 0 || mag.y != 0 || mag.z != 0)
	{
		normalise = qfp_fsqrt(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
		mag.x /= normalise;
		mag.y /= normalise;
		mag.z /= normalise;
	}

	/* 计算磁力计投影到物体坐标上的各个分量 */
	hx = 2.0f*mag.x*(0.5f - q2q2 - q3q3) + 2.0f*mag.y*(q1q2 - q0q3) + 2.0f*mag.z*(q1q3 + q0q2);
	hy = 2.0f*mag.x*(q1q2 + q0q3) + 2.0f*mag.y*(0.5f - q1q1 - q3q3) + 2.0f*mag.z*(q2q3 - q0q1);
	hz = 2.0f*mag.x*(q1q3 - q0q2) + 2.0f*mag.y*(q2q3 + q0q1) + 2.0f*mag.z*(0.5f - q1q1 - q2q2);
	bx = qfp_fsqrt((hx*hx) + (hy*hy));
	bz = hz;

	/* 计算加速度计投影到物体坐标上的各个分量 */
	vx = 2.0f*(q1q3 - q0q2);
	vy = 2.0f*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;

	/* 处理过后的磁力计新分量 */
	wx = 2.0f*bx*(0.5f - q2q2 - q3q3) + 2.0f*bz*(q1q3 - q0q2);
	wy = 2.0f*bx*(q1q2 - q0q3) + 2.0f*bz*(q0q1 + q2q3);
	wz = 2.0f*bx*(q0q2 + q1q3) + 2.0f*bz*(0.5f - q1q1 - q2q2);

	/* 叉积误差累计，用以修正陀螺仪数据 */
	ex = (acc.y*vz - acc.z*vy) + (mag.y*wz - mag.z*wy);
	ey = (acc.z*vx - acc.x*vz) + (mag.z*wx - mag.x*wz);
	ez = (acc.x*vy - acc.y*vx) + (mag.x*wy - mag.y*wx);

	/* 互补滤波 PI */
	exInt += ex * Ki * halfT;
	eyInt += ey * Ki * halfT;
	ezInt += ez * Ki * halfT;
	gyro.x += Kp*ex + exInt;
	gyro.y += Kp*ey + eyInt;
	gyro.z += Kp*ez + ezInt;

	/* 使用一阶龙格库塔更新四元数 */
	q0 += (-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z) * halfT;
	q1 += ( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y) * halfT;
	q2 += ( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x) * halfT;
	q3 += ( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x) * halfT;

	/* 对四元数进行归一化处理 */
	normalise = qfp_fsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 /= normalise;
	q1 /= normalise;
	q2 /= normalise;
	q3 /= normalise;

	/* 由四元数求解欧拉角 */
	position->pitch = -asinf(-2*q1*q3 + 2*q0*q2); // * RAD2DEG;	//pitch
	position->roll = qfp_fatan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1); // * RAD2DEG;	//roll
	position->yaw = qfp_fatan2(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1); // * RAD2DEG;	//yaw
}

void update_estimate(const MpuData *data, Position *position) {
	acc.x = data->acceleration[X_AXIS] / ACC_SENSITIVITY;
	acc.y = data->acceleration[Y_AXIS] / ACC_SENSITIVITY;
	acc.z = data->acceleration[Z_AXIS] / ACC_SENSITIVITY;

	gyro.x = data->gyro[X_AXIS] / GYRO_SENSITIVITY;
	gyro.y = data->gyro[Y_AXIS] / GYRO_SENSITIVITY;
	gyro.z = data->gyro[Z_AXIS] / GYRO_SENSITIVITY;

	mag.x = data->magnetic[MAG_X_AXIS] / 1000.f;
	mag.y = data->magnetic[MAG_Y_AXIS] / 1000.f;
	mag.z = -data->magnetic[MAG_Z_AXIS] / 1000.f;

	imuUpdate(position);
}
